#include <iostream>
#include <climits>
#include <vector>
#include <LEDA/numbers/fp.h>
#include <LEDA/graphics/graphwin.h>
#include <LEDA/graphics/color.h>
#include <LEDA/core/set.h>
#include <LEDA/core/queue.h>
#include <LEDA/core/p_queue.h>
#include <LEDA/graphics/window.h>
#include <LEDA/graph/graph.h>
#include <LEDA/graph/node_set.h>
#include <algorithm>
#include <cmath>
#include "control.h"


using namespace leda;


double getEuclideanDist(GraphWin &gw, node &v, node &u)
{
	return gw.get_position(v).distance(gw.get_position(u));
}

vector getDiffVector(GraphWin &gw, node &v, node &u)
{
	return gw.get_position(v) - gw.get_position(u);
}

vector getUnitVector(GraphWin &gw, node &v, node &u)
{
	double dist = getEuclideanDist(gw, v, u);

	if (dist == 0) {
		vector v(1.0 , 1.0);
		return v;
	}
	else
		return getDiffVector(gw, v, u) / dist;
}

// The repulsive force that v experiences from u
vector getRepulsiveForce(GraphWin &gw, node &v, node &u, double C_0)
{
	double dist = getEuclideanDist(gw, v, u);
	if (dist == 0)
		return getUnitVector(gw, v, u) *  C_0;


	if (dist != 0.0 && !isnan(dist) && !isinf(( C_0 / pow(dist, 2) ))) {
		return getUnitVector(gw, v, u) *  ( C_0 / pow(dist, 2) );
	} else {
		return getUnitVector(gw, v, u) *  C_0;
	}
}

// The spring force experienced by v via the spring from u
vector getSpringForce(GraphWin &gw, node &v, node &u, double L, double C_1)
{
	return getUnitVector(gw, v, u) * (-C_1 * ( getEuclideanDist(gw, v, u) - L));
}


//////// NO VISUALIZATION FUNCTIONS ////////////////////////////////

double no_vis_getEuclideanDist(node_array<point> &current_position, node &v, node &u)
{
	return current_position[v].distance(current_position[u]);
}

vector no_vis_getDiffVector(node_array<point> &current_position, node &v, node &u)
{
	return current_position[v] - current_position[u];
}

vector no_vis_getUnitVector(node_array<point> &current_position, node &v, node &u)
{
	double dist = no_vis_getEuclideanDist(current_position, v, u);

	if (dist == 0) {
		vector v(1.0 , 1.0);
		return v;
	}
	else
		return no_vis_getDiffVector(current_position, v, u) / dist;
}

// The repulsive force that v experiences from u
vector no_vis_getRepulsiveForce(node_array<point> &current_position, node &v, node &u, double C_0)
{
	double dist = no_vis_getEuclideanDist(current_position, v, u);
	if (dist == 0)
		return no_vis_getUnitVector(current_position, v, u) *  C_0;


	if (dist != 0.0 && !isnan(dist) && !isinf(( C_0 / pow(dist, 2) ))) {
		return no_vis_getUnitVector(current_position, v, u) *  ( C_0 / pow(dist, 2) );
	} else {
		return no_vis_getUnitVector(current_position, v, u) *  C_0;
	}
}
/////////////////////////////////////////////////////////////////////////

// The spring force experienced by v via the spring from u
vector no_vis_getSpringForce(node_array<point> &current_position, node &v, node &u, double L, double C_1)
{
	return no_vis_getUnitVector(current_position, v, u) * (-C_1 * ( no_vis_getEuclideanDist(current_position, v, u) - L));
}

// dfs for checking # of connected nodes
void dfs(node parent, node v, graph &g, GraphWin &gw, node_array<int> &dfsnum, int &akt , node_set &unreached_nodes) {

	unreached_nodes.del(v);
	dfsnum[v] = akt++;
    edge e;
    forall_inout_edges(e, v) {
        node w = g.opposite(v, e);
        if (w != parent) {
            if (dfsnum[w] < 0) {
                dfs(v, w, g, gw, dfsnum, akt , unreached_nodes);
            }
        }
    }
}

// check if the graph is connected or not
node make_connected(graph &g, GraphWin &gw)
{
    node_array<int> dfsnum(g, -1);
    node v = g.first_node();
    int akt = 0;

    node_set unreached_nodes(g);
    forall_nodes(v,g) unreached_nodes.insert(v);

    node_set comp(g);

    while(!unreached_nodes.empty()) {
    	v = unreached_nodes.choose();
    	// run dfs for checking # of connected nodes
    	dfs(v, v, g, gw, dfsnum, akt , unreached_nodes);

    	comp.insert(v);
    }

    if (comp.size() > 1) {
    	std::cout << "Graph not connected! Adding new node and connect it to one node of each component!" << std::endl;
    	// creating new vertex and we color it red
    	node w = g.new_node();
    	gw.set_color(w, red);
    	gw.set_label(w,"M");

    	// set new vertex position to middle
    	point middle = point((gw.get_xmax() - gw.get_xmin()) / 2 ,(gw.get_ymax() - gw.get_ymin()) / 2);
    	gw.set_position(w, middle);
    	control_wait(0.5);

    	// add edges
    	node v;
    	while (!comp.empty()) {
    		v = comp.choose();
    		gw.new_edge(v,w);
    		comp.del(v);
    	}

    	control_wait(0.5);
    	return w;

    }

   	// return null if no node was added
	return NULL;
}


void springembedder(graph &g, GraphWin &gw)
{
	// The constants:
	const double C_0 = 70000.0; 	// Don't push too hard (it may add up with several nodes in the area)
	const double C_1 = 1.0;			// push / pull pretty hard
	const double L = 120;			// relatively long springs, else it will spring back too hard in big graphs
	const double D = 180;			// Only push if we really have to
	const double DELTA = 0.60;		// Since the springs are tight soften the effect


	//iteration number
	int i = 0;

	//global force in this iteration
	vector global_force;

	while ( (i < 5) || (i < g.number_of_nodes() / 1))
	{
		//initialize global force vector to 0,0
		global_force = vector(0,0);

		//init force vectors
		node_array<vector> repulsive_forces(g, vector(0,0));
		node_array<vector> spring_forces(g, vector(0,0));
		node_array<vector> influencing_forces(g, vector(0,0));

		/*first loop for computing the influencing force vectors for each node*/
		node u;
		forall_nodes(u, g) {

			if (gw.get_user_label(u) == "M") {
				continue;
			}
			// Compute repulsive force, iterate through all nodes
			node v;


			forall_nodes(v, g) {
				//skip node M to prevent empty middle
				if (gw.get_user_label(v) == "M")
					continue;

				// Check clipping, and that the node is not itself
				if ((getEuclideanDist(gw, u, v) < D) && (u != v)) {
					repulsive_forces[u] += getRepulsiveForce(gw, u, v, C_0);
				}
			}

			// Compute spring forces, iterate only adjacent nodes
			node adj_n;
			forall_adj_nodes(adj_n, u) {
				spring_forces[u] += getSpringForce(gw, u, adj_n, L, C_1);
			}

			influencing_forces[u] = repulsive_forces[u] + spring_forces[u];


			point pos = gw.get_position(u) + DELTA * influencing_forces[u];

			// set position and update graph
			gw.set_position(u, pos);
			global_force += influencing_forces[u];
		}

		// resize window according to the
		gw.zoom_graph();
		std::cout << "step " << i << " GlobalForce:"<< global_force << std::endl;
		i++;
	}
}


void no_vis_springembedder(graph &g, GraphWin &gw)
{
	// The constants:
	const double C_0 = 70000.0; 	// Don't push too hard (it may add up with several nodes in the area)
	const double C_1 = 1.0;			// push / pull pretty hard
	const double L = 120;			// relatively long springs, else it will spring back too hard in big graphs
	const double D = 180;			// Only push if we really have to
	const double DELTA = 0.60;		// Since the springs are tight soften the effect


	//iteration number
	int i = 0;

	//global force in this iteration
	vector global_force;

	//init current position array
	node_array<point> current_position(g, point(0,0));

	// init positions of the nodes
	node u;
	forall_nodes(u, g) {
		current_position[u] = gw.get_position(u);
	}

	while ( (i < 5) || (i < g.number_of_nodes() / 1))
	{
		//initialize global force vector to 0,0
		global_force = vector(0,0);

		//init force vectors
		node_array<vector> repulsive_forces(g, vector(0,0));
		node_array<vector> spring_forces(g, vector(0,0));
		node_array<vector> influencing_forces(g, vector(0,0));


		node u;
		/*first loop for computing the influencing force vectors for each node*/
		forall_nodes(u, g) {

			if (gw.get_user_label(u) == "M") {
				continue;
			}
			// Compute repulsive force, iterate through all nodes
			node v;


			forall_nodes(v, g) {
				//skip node M to prevent empty middle
				if (gw.get_user_label(v) == "M")
					continue;

				// Check clipping, and that the node is not itself
				if ((no_vis_getEuclideanDist(current_position, u, v) < D) && (u != v)) {
					repulsive_forces[u] += no_vis_getRepulsiveForce(current_position, u, v, C_0);
				}
			}

			// Compute spring forces, iterate only adjacent nodes
			node adj_n;
			forall_adj_nodes(adj_n, u) {
				spring_forces[u] += no_vis_getSpringForce(current_position, u, adj_n, L, C_1);
			}

			influencing_forces[u] = repulsive_forces[u] + spring_forces[u];


			//point pos = gw.get_position(u) + DELTA * influencing_forces[u];
			current_position[u] = current_position[u] + DELTA * influencing_forces[u];

			// set position and update graph
			// gw.set_position(u, pos);
			global_force += influencing_forces[u];
		}

		// resize window according to the graph
		//gw.zoom_graph();
		std::cout << "step " << i << " GlobalForce:"<< global_force << std::endl;
		i++;
	}

	// set position and update graph
	forall_nodes(u, g) {
		gw.set_position(u, current_position[u]);
	}

	// resize window according to the graph
	gw.zoom_graph();
}

int main(int argc, char *argv[]) {

	window *MaxMinWindowP;
    GraphWin gw(800, 600);
    gw.display();
    create_control();
    gw.set_directed(false);

    if (argc > 1)
        gw.read(argv[1]);
    gw.edit();
    graph &g = gw.get_graph();
    if (g.number_of_nodes() == 0) {
        gw.close(); destroy_control();
        exit(1);
    }

    // Reading the running type, maximal or minimal weighted matching
    MaxMinWindowP = new leda::window(260, 40, "Visualization?");
    MaxMinWindowP->buttons_per_line(2);
    MaxMinWindowP->button("Yes",  1);
    MaxMinWindowP->button("No",  2);
    MaxMinWindowP->display(0, 120);
    int running_type = MaxMinWindowP->read_mouse();
    MaxMinWindowP->close();

    node unconnected = make_connected(g, gw);

	// start spring layout algorithm with an extra vertex
    // with or without visualization
    if (running_type == 1){ 	//With visualization
    	springembedder(g, gw);
    }
    else if (running_type == 2){	//Without visualization
    	no_vis_springembedder(g, gw);
    }

    if (unconnected != NULL) {
    	std::cout << "Removing extra vertex!" << std::endl;
    	// remove extra vertex
    	gw.del_node(unconnected);
    	control_wait(0.5);
    }

    gw.acknowledge("Ready!");
    gw.edit();
    gw.close();
    destroy_control();
    exit(0);
}
